/*
 * Copyright 2012 Marcin Blazejewski
 *
 * This file is part of Ground0.
 *
 * Ground0 is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Ground0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Ground0. If not, see <http://www.gnu.org/licenses/>.
 *
 */


#include "collision.hpp"
#include "geometry.hpp"
#include "midlev.hpp"
#include "quat.hpp"
#include "stuff.hpp"

#include <list>
#include <vector>
#include <fstream>
#include <string.h>


static bool test_sphere_sphere(CollisionSphere &i1, CollisionSphere &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2);
static bool test_sphere_quad(CollisionSphere &i1, CollisionQuad &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2);
static bool test_quad_quad(CollisionQuad &i1, CollisionQuad &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2);

static int side(CollisionQuad &q, Vector &D, Vector &P);

/******************************************************************************/
/* Collision Sphere */
/******************************************************************************/

void CollisionSphere::load(std::istream &s) {
    s.read((char*)&center.x, sizeof(int));
    s.read((char*)&center.y, sizeof(int));
    s.read((char*)&center.z, sizeof(int));
    s.read((char*)&radius, sizeof(float));
}

void CollisionSphere::save(std::ostream &s) {
    s.write((char*)&center.x, sizeof(int));
    s.write((char*)&center.y, sizeof(int));
    s.write((char*)&center.z, sizeof(int));
    s.write((char*)&radius, sizeof(float));
}

/******************************************************************************/
/* Collision Model */
/******************************************************************************/

const char CollisionModel::id[] = "GR0CM";
const unsigned char CollisionModel::version = 2;

CollisionModel::CollisionModel() {
    CollisionSphere *cs = new CollisionSphere();
    cs->radius = 1.0;
    nodes.push_back(cs);
}

CollisionModel::~CollisionModel() {
    list<CollisionNode*>::iterator i;
    for (i = nodes.begin(); i != nodes.end(); i++) {
        delete *i;
    }
}


void CollisionModel::load(std::istream &s) {
    nodes.clear();

    char r_id[sizeof(id)];
    unsigned char r_ver = 0xFF;
    
    s.read(r_id, sizeof(id));
    s.read((char*)&r_ver, sizeof(version));

    if (strncmp(id, r_id, sizeof(id)) != 0) throw string("unknown file");
    if (r_ver > version) throw string("unsupported file version");

    int n;
    s.read((char*)&n, sizeof(int));
    for (int i = 0; i < n; i++) {
        CollisionSphere *cs = new CollisionSphere();
        cs->load(s);
        nodes.push_back(cs);
    }

    /*
    for (int r = 0; r < 3; r++) {
        s.read((char*)I_inv[r], sizeof(float) * 3);
    }
    */
}


void CollisionModel::save(std::ostream &s) {
    int n = nodes.size();
    s.write(id, sizeof(id));
    s.write((char*)&version, sizeof(version));
    s.write((char*)&n, sizeof(int));

    list<CollisionNode*>::iterator i;
    for (i = nodes.begin(); i != nodes.end(); i++) {
        (*i)->save(s);
    }
    /*
    for (int r = 0; r < 3; r++) {
        s.write((char*)I_inv[r], sizeof(float) * 3);
    }
    */
}

bool CollisionModel::test(Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2) {
    static list<CollisionNode*>::iterator i1;
    static list<CollisionNode*>::iterator i2;
    
    for (i1 = nodes.begin(); i1 != nodes.end(); i1++) {
        for (i2 = s2.cm->nodes.begin(); i2 != s2.cm->nodes.end(); i2++) {
            switch((*i1)->node_type) {
            case CN_SPHERE:
                switch ((*i2)->node_type) {
                case CN_SPHERE: if (test_sphere_sphere((CollisionSphere&)**i1, (CollisionSphere&)**i2, s1, s2, n, p1, p2)) return true;
                case CN_QUAD: if (test_sphere_quad((CollisionSphere&)**i1, (CollisionQuad&)**i2, s1, s2, n, p1, p2)) return true;
                }
            
            case CN_QUAD:
                switch ((*i2)->node_type) {
                case CN_SPHERE: if (test_sphere_quad((CollisionSphere&)**i2, (CollisionQuad&)**i1, s2, s1, n, p2, p1)) return true;
                case CN_QUAD: if (test_quad_quad((CollisionQuad&)**i1, (CollisionQuad&)**i2, s1, s2, n, p1, p2)) return true;
                }
            }
        }
    }

    return false;
}



static bool test_sphere_sphere(CollisionSphere &i1, CollisionSphere &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2) {
    static Vector na, nb; // centers of collision spheres for this (a) and other object (b)

    // calc collision sphere centers
    na = s1.pos + i1.center * s1.orientation;
    nb = s2.pos + i2.center * s2.orientation;
    // calc squared distance between collision sphere centers
    float d2 = na.distance2_to(nb);
    // collision distance
    float rsum2 = (i1.radius + i2.radius) * (i1.radius + i2.radius);
    if (d2 > rsum2) return false;
    
    if (p1 && p2 && n) {
        // collision normal = vector between collision sphere centers
        *n = nb - na;
        n->normalize();
        
        // collision points on each sphere:
        // p = sphere center + sphere radius along colision normal
        *p1 = na + (*n) * i1.radius;
        *p2 = nb - (*n) * i2.radius; // normal points from a -> b, need to reverse for b
    }
    
    return true;


}
static bool test_sphere_quad(CollisionSphere &i1, CollisionQuad &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2) {
    return false;
}

static bool test_quad_quad(CollisionQuad &i1, CollisionQuad &i2, Stuff &s1, Stuff &s2, Vector *n, Vector *p1, Vector *p2) {
    //Vector v = s.pos + cq->v[j] * s.orientation;
    static Vector v1[4];
    static Vector v2[4];
    int j;

    for (int j = 0; j < 4; j++) {
        v1[j] = s1.pos + i1.v[j] * s1.orientation;
        v2[j] = s2.pos + i2.v[j] * s2.orientation;
    }

    face_normal(v1[0], v1[1], v1[2], *n);
    *p1 = v1[0];

    return (side(i2, *n, v1[0]) == 0);
}




static int side(CollisionQuad &q, Vector &D, Vector &P) {
    int positive = 0;
    int negative = 0;
    int i;
    float t;
    
    for (i = 0; i < 4; i++) {
        Vector tmp = q.v[i] - P;
        t = D.dot(tmp);
        if (t < 0) negative++;
        else if (t > 0) positive++;

        if (positive && negative) return 0;
    }

    return (positive ? 1 : -1);
}
